/*
The surface-physics project: http:// code.google.com/p/surface-physics/
Adds real world physics based on the Bullet physics engine, to bases in the Orbiter space
flight simulation software.

Written by Abhijit Nandy
License : Apache License 2.0  

Bump_VehicleCollider.cpp
Bump_VehicleCollider implementation.


*/


#include "Bump_VehicleCollider.h"

#include "../BulletBase/Bump_CollisionShape.h"
#include "../Terrain/Bump_TerrainPatchOru.h"
#include "BulletBaseVehicle.h"


Registrar Bump_VehicleCollider::registrar("VehicleCollider", Bump_VehicleCollider::create);

// Required to simulate friction as car chassis not in actual contact with ground
// TODO: This should be specific to the terrain, not a vehicle property
const float Bump_VehicleCollider::DEFAULT_FRICTION_BRAKING_FORCE = 100.;

// This may need to be adjusted later - negate it based on y-axis orientation wrt base ?
const float Bump_VehicleCollider::TORQUE_MULT_FACTOR = 50.;


/**
 * Constructor for this derived class
 */
Bump_VehicleCollider::Bump_VehicleCollider():
        ground(NULL),
        mColliderState(CS_UNINITIALIZED),
        collisionCount(0),
        mKinBodyActive(false),
        mDynBodyActive(false),
        mCollisionMeshActive(false),
        mHoverEnginesFiredForFlight(false),
        mVehicleAttached(false),
        mDefaultFlags(0),
        mVehicleCreated(true),
        mGroundPatchCreated(false)
{
    deleteIfInactive = false;
    strcpy(className, "VesselCollider");

    // Vehicle
    rbCollider = NULL;
    vehicle = NULL;


    linearVel  = btVector3(BTZEROVECTOR);
    angularVel = btVector3(BTZEROVECTOR);

    mCollisionInfo.resetCollisionInfo();
    mCollisionInfo.strObjectName = strObjectName;

    mPostStepFrequency = 0;

    //oapiWriteLogV("Bump_VehicleCollider : prestep : %d ", mPreStepFrequency);

}


/**
 * Destructor, does nothing currently
 */
Bump_VehicleCollider::~Bump_VehicleCollider()
{

    // If a collider is active and sim is ending then we must destroy the physics object now
    if (mVehicleCreated) {
    int rv = destroyBulletRigidBodyVehicle();
    if(rv)
        oapiWriteLogV("Bump_VehicleCollider::~Bump_VehicleCollider: ERROR : Destroying vehicle failed %s",
                strObjectName);
    }

    // Kept around till the end so it does not have to be recreated everytime state changes
    // Both kinematic and dynamic vehicle share this
    delete ground;

   // oapiWriteLogV("Bump_VehicleCollider::~Bump_VehicleCollider: Destroyed %s", strObjectName);
}


/**
 * Initialize a base attachment.
 * Creates a new BulletBase Attachment and inserts it in the Orbiter world & Bullet World
 *
 * The search or create functionality provided here has to be part of this derived class as
 * for the other derived class, there is no search required(Meshes must be created always)
 *
 * A attachment needs to be added to the BulletBase. There can be at most MAX_OBJECTS number of attachments
 * The check for whether the number of attachments is < MAX_OBJECTS is already done at
 * Bump_BulletBase::readCreatePhysicsFileObject()
 *
 * Also a single attachment point needs to be created in the vessel that will be attached.
 *
 * Both attachment points may need to be deleted midway(use Orbiter's functions)
 *
 *
 *
 * TODO: Currently only searches and attaches, no create.
 * TODO: Check if vessel attachment point already exists, also remove attachment point whenever detached
 *
 */
int
Bump_VehicleCollider::clbkSetClassCaps(InitialObjectData& initialData)
{
    int rv;
	double lng, lat, rad;

    // This call MUST be made before anything else, sets base
    Bump_BaseObject::clbkSetClassCaps(initialData);

    if (initialData.pVesselCollDef) {
        mCollDef = initialData.pVesselCollDef;
    }
    else {
        oapiWriteLogV("Bump_VehicleCollider::clbkSetClassCaps: ERROR :Invalid Collision Def for %s",
                        strObjectName);
        return 6;
    }

    //oapiWriteLogV("Bump_VehicleCollider::clbkSetClassCaps: Initializing %s", strObjectName);

    // If the object ref is valid go ahead and get the VESSEL ptr, its needed BEFORE attaching
    if(hObjThis){

        vCollider = (VESSEL3*)oapiGetVesselInterface(hObjThis);
        vCollider->GetEquPos(lng, lat, rad);


        mass = (btScalar)vCollider->GetMass();
        vesselRadius = (btScalar)vCollider->GetSize();
    }
    else{
        oapiWriteLogV("Bump_VehicleCollider::clbkSetClassCaps: ERROR :The hObj passed (%p) wasn't valid for %s",
                hObjThis, strObjectName);
        return 1;
    }

    // Choose the proper terrain generator as picked by the base.
    switch(bumpBase->terrainGenerator){
    case TT_PLANETCURVE:
        ground = new Bump_TerrainPatch(16, 16, 0.000002f, 0.000002f, 0.000010f, 0.000010f, 10.f, bumpBase);
        break;
    case TT_ORULEX:
        ground = new Bump_TerrainPatchOru(16, 16, 0.000002f, 0.000002f, 0.000010f, 0.000010f, 10.f, bumpBase);
        break;
    case TT_D3D11:
        break;
    default:
        oapiWriteLogV("Bump_VehicleCollider::clbkSetClassCaps: ERROR : There is no terrain generator specified for %s !!", strObjectName);
        // No terrain!!, physics controlled objects will fall through, should never happen !
        break;
    }

    if(!ground){
        oapiWriteLogV("Bump_VehicleCollider::clbkSetClassCaps: ERROR : Could not add ground for %s", strObjectName);
        return 3;
    }

    // Create terrain patch from initial equatorial position of vessel
    rv = ground->createPatch(lng, lat);
    if (rv) {
        oapiWriteLogV("Bump_VehicleCollider::init: ERROR : Could not add ground for %s", strObjectName);
        return 4;
    }

    // Collision shapes need only be got once as they involve file reads & may be expensive
    // This may need to be pushed to the VesselDef Cache so they can be shared
    rv = createCollisionShapes();
    if (rv) {
        oapiWriteLogV("Bump_VehicleCollider::init: ERROR : Could not create collision shapes for %s", strObjectName);
        return 5;
    }

    //  Check if Orbiter wheel animation is enabled
    mOrbiterWheelAnimationEnabled = true;
    for (int i = 0; i < mCollDef->numWheels; ++i)
        if (!mCollDef->isWheelInfoSet[i])
            mOrbiterWheelAnimationEnabled = false;

    // Attachment and Bullet body creation will happen later

    // TODO: Must check here if gear up/down
/*    rv = createBulletRigidBodyVehicle();
    if(rv){
        oapiWriteLogV("Bump_VehicleCollider::Bump_VehicleCollider: ERROR : Creating vehicle failed %s",
                strObjectName);
    }
    mVehicleCreated = true;*/



    return 0;
}


/**
 * Handle orbiter regime in post step  - after orbiter sim step so we capture
 * updated vessel position which is more in sync with the rendering mesh position
 */
int
Bump_VehicleCollider::clbkPostStep(double SimT, double SimDT, double MJD)
{
	/*btTransform transVessel1;
	getVesselTransformInBaseFrame(transVessel1);
	btVector3 v = transVessel1.getOrigin();
	oapiWriteLogV("Bump_VehicleCollider::clbkPostStep: %s ORIGIN %f, %f, %f", strObjectName, BT3ARGS(v));
*/


    switch(mColliderState){
        case CS_ORBITER_REGIME:
        {
            handleOrbiterRegime();
            break;
        }

        default:
            break;
    }

    // Prepare for next iteration's collision detection
    mCollisionInfo.resetCollisionInfo();

    return 0;
}


/**
 * This is basically a state machine. A vessel that is close to a base can collide
 * with another object which is present in the physics world. The vessel is initially
 * put into a state where its under Orbiter's control - as usual. If it collides with
 * terrain or another object it goes into a state which is basically Bullet control.
 * When the collision is done then we switch state back to Orbiter control.
 */
int
Bump_VehicleCollider::clbkPreStep(double SimT, double SimDT, double MJD)
{

    // oapiWriteLogV("Bump_VehicleCollider::poststep: The next sim step will advance the sim by %f secs", SimDT);

	//oapiWriteLogV("Ground:%p, box:%p", ground->mrbTerrainPatch, ground->mrbSupportPlate);



    switch(mColliderState){

        // This state is important for deleting this object and also for raising landed ships
        // above terrain(Orbiter puts them on the planet surface !)
        case CS_UNINITIALIZED:
        {
            double lng, lat, rad;

            // Get vessel position in both co-ord systems
            vCollider->GetEquPos(lng, lat, rad);

            // Get exact distance to Touch down points(not CoG) which = rad - cogElev
            cogElev = vCollider->GetCOG_elev();
            rad -= cogElev;

            // delete this collider if vessel beyond max distance, else we can proceed with attaching
            // this may need more work
            if(rad > bumpBase->maxTerrainRadialDist)
                deleteMe = true;
            else /*if ( !bumpBase->mFrozen)*/
                mColliderState = CS_ORBITER_REGIME;

            // sprintf(oapiDebugString(), "CS_UNINITIALIZED; rad : %f ", rad);


            break;
        }


        // Orbiter regime handled in clbkPostStep()


        // Activate the vessel in the Orbiter simulation by firing the hover thrusters
        case CS_PRE_BULLET_REGIME:
        {
        	if (mHoverEnginesFiredForFlight) {
				// Shut them
				vCollider->SetThrusterGroupLevel(vCollider->GetThrusterGroupHandle(THGROUP_HOVER), 0.f);
				mHoverEnginesFiredForFlight = false;
				mColliderState = CS_BULLET_REGIME;
			}
            else {
                vCollider->SetThrusterGroupLevel(vCollider->GetThrusterGroupHandle(THGROUP_HOVER), 0.3f);
                mHoverEnginesFiredForFlight = true;
            }

        	/*oapiWriteLogV("Bump_VehicleCollider::clbkPreStep: PBR %s", strObjectName);
            sprintf(oapiDebugString(), "PBR %s", strObjectName);*/

            break;
        }


        case CS_BULLET_REGIME:
        {
            handleBulletRegime();
            break;
        }


        case CS_TIMEWARP:
        {
            handleTimeWarp();
            break;
        }

        case CS_FROZEN_ORBITER:
		{
			handleFrozenOrbiter();
			break;
		}


        case CS_FROZEN_BULLET:
        {
            handleFrozenBullet();
            break;
        }


        default:
            break;
    } // end switch()


    return 0;
}


/**
 * Transfers Orbiter vehicle state to kinematic object continuously
 * Used to push away any colliding objects
 * Any collision causes a transition to the Bullet regime and the kinematic object to be replaced
 * with a dynamic rigid body under bullet control.
 */
int
Bump_VehicleCollider::handleOrbiterRegime()
{
    /*oapiWriteLogV("Bump_VehicleCollider::handleOrbiterRegime: Entered Orbiter regime mColliderState : %s %d, mKinBodyActive : %d",
    		strObjectName, mColliderState, mKinBodyActive);*/

    btScalar m[16];

    // Insert the physics kinematic body, no need to remove vehicle, it should be already gone!
    // removed in BULLET_REGIME itself when leaving that state
    if(!mKinBodyActive){
    	createBulletRigidBodyVehicle(true);
        mKinBodyActive = true;

       /* oapiWriteLogV("Bump_VehicleCollider::handleOrbiterRegime:Exit at create %s, %d, mKinBodyActive: %d",
                           		strObjectName, mColliderState, mKinBodyActive);*/
        return 0;
    }

    // -------------------- Do freeze checks right here  after rb created -----------------------
    if ( bumpBase->mFrozen ) {
    	mColliderState = CS_FROZEN_ORBITER;

		// Must attach orbiter vessel
		// Terrain data is potentially gone next iteration.
		//oapiWriteLogV("Bump_VehicleCollider::handleOrbiterRegime: Moving %s to CS_FROZEN_ORBITER", strObjectName);
		handleFrozenOrbiter();
		return 0;
    }

    btTransform transVessel;
    getVesselTransformInBaseFrame(transVessel);
    rbCollider->getMotionState()->setWorldTransform(transVessel);

    // Get velocities - bullet will set the velocity from kinematic info
    linearVel  = rbCollider->getLinearVelocity();
    angularVel = rbCollider->getAngularVelocity();

    // Get exact distance to Touch down points(not CoG) which = rad - cogElev
    // cogElev can change based on gear up/down
	double lng, lat, rad;
    vCollider->GetEquPos(lng, lat, rad);
    cogElev = vCollider->GetCOG_elev();
    rad -= cogElev;

    for(CollisionInfo::CollisionMap::iterator collMapIter =  mCollisionInfo.collMap.begin();
		collMapIter != mCollisionInfo.collMap.end(); collMapIter++){
		/*oapiWriteLogV("isChassisOnlyInPlanetSphereContact : %s Colliding with %p",
											strObjectName, collMapIter->first);*/
	}

    // If colliding with ground patch and Orbiter reports planetary sphere contact,
    // stay in ORBITER_REGIME by falsing any collision detection. We want Orbiter
    // to handle planetary sphere collision of chassis
	if ( mCollisionInfo.colliding && isChassisOnlyInPlanetSphereContact(mCollisionInfo) ) {
		mCollisionInfo.colliding = false;

		/*for(CollisionInfo::CollisionMap::iterator collMapIter =  mCollisionInfo.collMap.begin();
			collMapIter != mCollisionInfo.collMap.end(); collMapIter++){
			oapiWriteLogV("Chassis is colliding with %p",
												strObjectName, collMapIter->first);
		}*/
	}

	//oapiWriteLogV("OR :1 mCollisionInfo.colliding:%d", mCollisionInfo.colliding);

    //  Now check if the wheels are colliding, OR with chassis collision info from nearphase
    //  Note: that m[] will be changed below
    for (int i=0; i < mCollDef->numWheels; i++) {

    	// For wheels if Orbiter reports planetary sphere collision & the ground patch is
		// in contact then vehicle wheels are in touch with sphere, let orbiter handle it.
		if (vehicle->getWheelInfo(i).m_raycastInfo.m_isInContact &&
            isWheelOnlyInPlanetSphereContact(vehicle->getWheelInfo(i).m_raycastInfo)
		) {
			mCollisionInfo.colliding |= 0;
			//oapiWriteLogV("OR :2 Wheel %d, mCollisionInfo.colliding:%d", i, mCollisionInfo.colliding);
		}
		else {
			// Something else, either wheel struck another object or wheels struck orulex
			// terrain high above planetary sphere.
			mCollisionInfo.colliding |=vehicle->getWheelInfo(i).m_raycastInfo.m_isInContact;
			//oapiWriteLogV("OR :3 Wheel %d, mCollisionInfo.colliding:%d", i, mCollisionInfo.colliding);
		}

        // synchronize the wheels with the (interpolated) chassis worldtransform
        vehicle->updateWheelTransform(i, true);

        btTransform transWheel = vehicle->getWheelInfo(i).m_worldTransform;
        transWheel.getOpenGLMatrix(m);

        btVector3 trueWheelPos = btVector3(m[12], m[13], m[14]) - bumpBase->simBoxOffset;

        // Place collision meshes in Orbiter window - DO NOT MOVE TO END of case
        // mCollisionInfo updated in this loop & must be right here
       /* btTransform transShape = transWheel;
        transShape.setOrigin(trueWheelPos);
        mptrCollMshWheel[i]->drawSimpleShape(transShape);*/
    }



    btVector3 knmOrigin = transVessel.getOrigin();

    if (oapiGetFocusObject() == hObjThis /*strcmp(strObjectName, "Collider1") == 0 */) {
        /*sprintf(oapiDebugString(), "CS_ORBITER_REGIME; %f %f, %f, "
            "count=%d, colliding:%d", BT3ARGS(knmOrigin), collisionCount, mCollisionInfo.colliding);*/
    }

   /* oapiWriteLogV("CS_ORBITER_REGIME: Knmorigin : %f %f, %f, vel: %f, %f, %f",
                  BT3ARGS(knmOrigin), BT3ARGS(linearVel));*/

    double distanceAboveTerrain = getDistanceAboveTerrain();


    // If physics engine signals a collision move to BULLET_REGIME and take over control
    // Occurrence of collisions is the sole impulse to transition to bullet regime
    // This allows for collisions at any distance above surface - NEEDS TESTING
    if ( mCollisionInfo.colliding ) {
		//oapiWriteLogV("OR :4 mCollisionInfo.colliding:%d", mCollisionInfo.colliding);

		destroyBulletRigidBodyVehicle();
		mKinBodyActive = false;

		// Do we really need to kick the vessel to life ?
		if (vCollider->GetFlightStatus() == 0) {
			mColliderState = CS_PRE_BULLET_REGIME;
			//oapiWriteLogV("OR : Moving %s to PBR", strObjectName);
		}
		else {
			mColliderState = CS_BULLET_REGIME;
			//oapiWriteLogV("OR : Moving %s to BR", strObjectName);
		}

	    /*oapiWriteLogV("Bump_VehicleCollider::handleOrbiterRegime:Exit at destroy %s, mColliderState: %d, mKinBodyActive: %d",
	            		strObjectName, mColliderState, mKinBodyActive);*/

		return 0;


    }    
    else if (rad > bumpBase->maxTerrainRadialDist) {
    	// Let go if vehicle beyond collision range of base
        mKinBodyActive = false;
        mColliderState = CS_UNINITIALIZED;   // this move back to CS_UNINITIALIZED may not be needed !
    }
	else {
		// Collisions were not continuous, reset counter
		collisionCount = 0;
	}


    // Update terrain - for detecting terrain collision in next iteration
    if (ground) {
    	/*btTransform transVessel1;
    	getVesselTransformInBaseFrame(transVessel1);
    	btVector3 v = transVessel1.getOrigin();*/
    	//oapiWriteLogV("%s ORIGIN %f, %f, %f", strObjectName, BT3ARGS(v));

        ground->updatePatch(lng, lat, rad);

        //btVector3 o =  ground->mrbTerrainPatch->getWorldTransform().getOrigin();
        //oapiWriteLogV("Ground : ORIGIN %f, %f, %f", BT3ARGS(o));
    }

    // Draw the chassis collision mesh - TODO: check if display is enabled
   /* btTransform transFinal = transVessel;
    transFinal.setOrigin(transVessel.getOrigin() - bumpBase->simBoxOffset); // this needs validation
    mptrCollMshChassis->drawCompoundShape(transFinal, compoundChassisShape);*/

    // This should be here, do not move to clbkPreStep() entry point, this will cause focus to be set on
    // a destroyed rigid body pointer when transitioning from orbiter to bullet regime leading to a crash !
    if(oapiGetFocusObject() == hObjThis) {
		//oapiWriteLogV("Bump_VehicleCollider::handleOrbiterRegime about to call clbkSetFocus: %p", rbCollider);
		clbkSetFocus();
	}

    /*oapiWriteLogV("Bump_VehicleCollider::handleOrbiterRegime: Exiting Orbiter regime %s, mColliderState %d, \
    		mKinBodyActive: %d", strObjectName, mColliderState, mKinBodyActive);*/

    return 0;
}


/**
 * This state does not apply any torques as that would imply more accurate MOI
 * Tradeoff : Torques have a negligible effect for
 * getting collision response. Thrusters are applied in Bullet. Any thruster torques
 * to avoid collision will be ineffective once objects have collided.
 */
int
Bump_VehicleCollider::handleBulletRegime()
{
   /* oapiWriteLogV("Bump_VehicleCollider::handleBulletRegime: %s Entry, mColliderState : %d, mDynBodyActive:%d",
    		strObjectName, mColliderState, mDynBodyActive);*/

    if(!mDynBodyActive){

    	createBulletRigidBodyVehicle(false);

        // Make the wheel animations - see createAnimations() comments
        if (mOrbiterWheelAnimationEnabled)
            createAnimations();

        mDynBodyActive = true;

        /*oapiWriteLogV("Bump_VehicleCollider::handleBulletRegime:Exit at create %s, %d, mDynBodyActive: %d",
                		strObjectName, mColliderState, mDynBodyActive);*/

        return 0;
    }

    //---------------- Test for state changes since dynamic vehicle created  ------------------

    // This check MUST BE HERE, DO NOT MOVE IT ANYWHERE ELSE !
    // base frozen check has priority as we will oscillate between time warp <-> frozen otherwise
    // Cannot move to time warp unless dynamic vehicle created !
    // If we enter switch-case at high warp from orbiter regime, then we will get a crash !
    if (bumpBase->mFrozen) {
        mColliderState = CS_FROZEN_BULLET;

        // Must attach orbiter vessel and freeze rigid body immediately.
        // Terrain data is potentially gone next iteration.
        //oapiWriteLogV("Bump_VehicleCollider::handleBulletRegime: Moving %s to CS_FROZEN_BULLET", strObjectName);
        handleFrozenBullet();
        return 0;
    }
    else if (Bump_BulletBase::mTimeWarpFactor > 1.f) {
        mColliderState = CS_TIMEWARP;
        //oapiWriteLogV("Bump_VehicleCollider::handleBulletRegime: Moving %s to time warp.", strObjectName);
        // No return here !, will continue below
    }


    // -----------------------------------------------------------------------------------------

    // Main world transform of the vehicle(in base frame)
    btScalar m[16];
    btTransform transVessel;
    rbCollider->getMotionState()->getWorldTransform(transVessel);
    transVessel.getOpenGLMatrix(m);

    // mTrueChassisPos is persisted as a class member & passed to transformVessel()
    btVector3 trueChassisPos = btVector3(m[12], m[13], m[14]) - bumpBase->simBoxOffset;

    // Apply thrust and update the wheels too
    double magnWeight, magnUpThrust;
    applyThrustOnDynamicVehicle(transVessel, trueChassisPos, magnWeight, magnUpThrust);

   /* btTransform transVessel2;
	getVesselTransformInBaseFrame(transVessel2);
	btVector3 v1 = transVessel2.getOrigin();
	oapiWriteLogV("Bump_VehicleCollider::handleBulletRegime: 2. %s ORIGIN %f, %f, %f", strObjectName, BT3ARGS(v1));*/


    // We move out of Bullet control if by any chance vessel is flung too far out
    double    distAboveTerrain = getDistanceAboveTerrain();

    // Get the bounding sphere
    btVector3 bsphCenter;
    float bsphRad;
    compoundChassisShape.getBoundingSphere(bsphCenter, bsphRad);

    //oapiWriteLogV("BR : 5. mCollisionInfo.colliding:%d", mCollisionInfo.colliding);

    // Transition to Orbiter only when no collisions happening
    if ( !mCollisionInfo.colliding ) {

    	transformVessel(trueChassisPos);

    	destroyBulletRigidBodyVehicle();

    	// Deletion must be before dynamic vehicle is destroyed
        if (mOrbiterWheelAnimationEnabled)
            deleteAnimations();

        mDynBodyActive = false;
        mColliderState = CS_ORBITER_REGIME;   // this move back to CS_UNINITIALIZED may not be needed !

        //oapiWriteLogV("Bump_VehicleCollider::handleBulletRegime: Moving %s to Orbiter Regime", strObjectName);

        // State change to Orbiter scheduled, do not update vessel
        // Vehicle has been destroyed

        /*oapiWriteLogV("Bump_VehicleCollider::handleBulletRegime:Exit at destroy %s, %d, mDynBodyActive: %d",
        		strObjectName, mColliderState, mDynBodyActive);*/

        return 0;

    }
    else if ( distAboveTerrain < (bsphRad * -2) ) {  // vehicle is below bounding sphere diameter

        // Check if vessel is no longer in flight
        // If so move to PRE-BULLET_REGIME, destroying this object
        //else inputs wont be recvd any longer.

       /* oapiWriteLogV("Bump_VehicleCollider::handleBulletRegime: %s below ground, pushing up, distanceAboveTerrain : %f",
                strObjectName, distAboveTerrain);*/
        placeDynamicVehicleAboveTerrain(transVessel);
    }

    // Apply transform to Orbiter only if no state change
    transformVessel(trueChassisPos);

    // This should be here, do not move to clbkPreStep() entry point, this will cause focus to be set on
    // a destroyed rigid body pointer when transitioning from orbiter to bullet regime leading to a crash !
    if(oapiGetFocusObject() == hObjThis) {
    	//oapiWriteLogV("Bump_VehicleCollider::handleBulletRegime about to call clbkSetFocus: %p", rbCollider);
    	clbkSetFocus();
    }

    /*if (strcmp(strObjectName, "Collider1") == 0 )
        sprintf(oapiDebugString(), "CS_BULLET_REGIME : magnUpThrust:%f,  magnWeight:%f, colliding=%d, rad:%f",
                        magnUpThrust, magnWeight, mCollisionInfo.colliding, rad);*/

    /*oapiWriteLogV("CS_BULLET_REGIME : magnUpThrust:%f,  magnWeight:%f, colliding=%d, rad:%f, dist:%f",
            magnUpThrust, magnWeight, mCollisionInfo.colliding, rad, distanceAboveTerrain);*/

    //oapiWriteLogV("Bump_VehicleCollider::handleBulletRegime: %s %d Exit, mDynBodyActive:%d",
    //		strObjectName, mColliderState, mDynBodyActive);

    return 0;
}


/**
 * At high time warps DefSetStateEx() is unable to keep a vessel stable above the planetary
 * sphere (on terrain). So its best to move to rigid positioning using attachment points for the
 * duration. Forces still need to be applied and its possible a vessel will take flight due to its
 * thrusters,
 *
 * Decided that we will not transition back to CS_ORBITER_REGIME directly. Can cause
 * complications. We go to CS_BULLET_REGIME and detect if we are already in flight and go to orbiter
 * regime from there if needed. This also helps us to restore animations from one point only.
 *
 * No need to complicate the state machine yet.
 *
 */
int
Bump_VehicleCollider::handleTimeWarp()
{
    //oapiWriteLogV("Bump_VehicleCollider::handleTimeWarp: %s Entry", strObjectName);

    // Attach the vessel and use attachment points for movement
    if(!mVehicleAttached){

        // Attach the vessel now
        int rv = attachOrbiterVESSEL();
        if(rv){
            oapiWriteLogV("Bump_VehicleCollider::handleTimeWarp: ERROR : Attaching Orbiter VESSEL failed %s", strObjectName);
            return 4;
        }

        mVehicleAttached = true;
    }

    if(oapiGetFocusObject() == hObjThis)
        clbkSetFocus();


    // Main world transform of the vehicle(in base frame)
    btScalar m[16];
    btTransform transVessel;
    rbCollider->getMotionState()->getWorldTransform(transVessel);
    transVessel.getOpenGLMatrix(m);

    btVector3 trueChassisPos = btVector3(m[12], m[13],    m[14]) - bumpBase->simBoxOffset;

    // Apply thrust and update the wheels too
    double magnWeight, magnUpThrust;
    applyThrustOnDynamicVehicle(transVessel, trueChassisPos, magnWeight, magnUpThrust);

    // We move out of Bullet control if by any chance vessel is flung too far out
    double distanceAboveTerrain = getDistanceAboveTerrain();

    btVector3 bsphCenter;
    float bsphRad;
    compoundChassisShape.getBoundingSphere(bsphCenter, bsphRad);

    if (Bump_BulletBase::mTimeWarpFactor < 10.f || bumpBase->mFrozen) {
        // When time warp finished move back to normal movement

        // Detach vessel
        int rv = detachOrbiterVESSEL();
        if(rv){
            oapiWriteLogV("Bump_VehicleCollider::handleTimeWarp: ERROR : Detaching Orbiter VESSEL failed %s", strObjectName);
            return 6;
        }

        // Transfer the dynamic vehicle state to Orbiter
        transformVessel(trueChassisPos);

        mVehicleAttached = false;
        mColliderState = CS_BULLET_REGIME;

        //oapiWriteLogV("CS_TIMEWARP : Moving to Bullet Regime");

        // MUST return as attachment point is gone now!
        return 0;
    }
    else if ( distanceAboveTerrain < (bsphRad * -2)  ) {
        oapiWriteLogV("CS_TIMEWARP : Collider below ground, pushing up, distanceAboveTerrain : %f", distanceAboveTerrain);
        placeDynamicVehicleAboveTerrain(transVessel);
    }

    // Move about using attachment points if no state change
    bumpBase->bb->SetAttachmentParams(mAHBase,
                     _V(BT3ARGS(trueChassisPos)),
                     _V(m[4], m[5], m[6]),                            //y axis
                     _V(m[8], m[9], m[10]));                        //z axis

   /* sprintf(oapiDebugString(), "CS_TIMEWARP : magnUpThrust:%f,  magnWeight:%f, colliding=%d",
                        magnUpThrust, magnWeight, mCollisionInfo.colliding);

    oapiWriteLogV("CS_TIMEWARP : %f, magnUpThrust:%f,  magnWeight:%f, colliding=%d, dist:%f",
            Bump_BulletBase::mTimeWarpFactor, magnUpThrust, magnWeight, mCollisionInfo.colliding, distanceAboveTerrain);*/

    return 0;

}


/**
 * Handle the frozen case, just attach and hold vessel in place till base is
 * unfrozen. This is similar to handling time warp except that we do not need to freeze the
 * Bullet rigid body as it no longer receives any input - VERIFY THIS
 */
int
Bump_VehicleCollider::handleFrozenOrbiter()
{
    //oapiWriteLogV("Bump_VehicleCollider::handleFrozenOrbiter: %s : Entry", strObjectName);

    // Attach the vessel and use attachment points for movement
    // The same attachment flag should be shared by this state and time warp, so we can detect attachment conflicts
    if(!mVehicleAttached){

        // Attach the vessel now
        int rv = attachOrbiterVESSEL();
        if(rv){
            oapiWriteLogV("Bump_VehicleCollider::handleFrozenOrbiter: ERROR : Attaching Orbiter VESSEL failed %s", strObjectName);
            return 1;
        }

        mVehicleAttached = true;

        // Reset animations - not visible anymore - visual is lost
        //removeCollisionShapes();
    }

    rbCollider->setActivationState(WANTS_DEACTIVATION);

    if (!bumpBase->mFrozen) {
        // When freeze finished & focus regained, move back to normal movement

        // Detach vessel
        int rv = detachOrbiterVESSEL();
        if(rv){
            oapiWriteLogV("Bump_VehicleCollider::handleFrozenOrbiter: ERROR : Detaching Orbiter VESSEL failed for %s", strObjectName);
            return 2;
        }

        // Add in the animations again as visual is regained
        //addCollisionShapes();

        mVehicleAttached = false;
        mColliderState = CS_ORBITER_REGIME;

        //oapiWriteLogV("Bump_VehicleCollider::handleFrozenOrbiter: Moving %s to OR", strObjectName);

        // MUST return as attachment point is gone now!
        return 0;
    }

   // oapiWriteLogV("Bump_VehicleCollider::handleFrozenOrbiter: %s Exit", strObjectName);
    return 0;

}


/**
 * Handle the frozen case, just attach and hold vessel in place till base is
 * unfrozen. This is similar to handling time warp except that we also freeze the
 * Bullet rigid body and transfer no motion data from bullet to orbiter.
 */
int
Bump_VehicleCollider::handleFrozenBullet()
{
    //oapiWriteLogV("Bump_VehicleCollider::handleFrozenBullet: %s : Entry", strObjectName);

    // Attach the vessel and use attachment points for movement
    // The same attachment flag should be shared by this state and time warp, so we can detect attachment conflicts
    if(!mVehicleAttached){

        // Attach the vessel now
        int rv = attachOrbiterVESSEL();
        if(rv){
            oapiWriteLogV("Bump_VehicleCollider::handleFrozenBullet: ERROR : Attaching Orbiter VESSEL failed %s", strObjectName);
            return 1;
        }

        mVehicleAttached = true;

        // Save the current velocity information
        linearVel  = rbCollider->getLinearVelocity();
        angularVel = rbCollider->getAngularVelocity();

        // Reset animations - not visible anymore - visual is lost
        //removeCollisionShapes();
    }

    // Also freeze the Bullet rigid body into its position if nothing is colliding with the chassis(do not check wheels)
    // Wheels will be colliding with terrain below but we still need to freeze.
    if (!mCollisionInfo.colliding) {
        rbCollider->setLinearVelocity(BTZEROVECTOR);
        rbCollider->setAngularVelocity(BTZEROVECTOR);
    }

    // Cut gravity and try disabling
    rbCollider->setGravity(BTZEROVECTOR);
    rbCollider->setActivationState(WANTS_DEACTIVATION);



    if (!bumpBase->mFrozen) {
        // When time warp finished move back to normal movement

        // Detach vessel
        int rv = detachOrbiterVESSEL();
        if(rv){
            oapiWriteLogV("Bump_VehicleCollider::handleFrozenBullet: ERROR : Detaching Orbiter VESSEL failed for %s", strObjectName);
            return 2;
        }

        //rbCollider->setCollisionFlags( mDefaultFlags );

        // Restore gravity and velocities
        rbCollider->setLinearVelocity(linearVel);
        rbCollider->setAngularVelocity(angularVel);
        rbCollider->setGravity(btVector3((btScalar)bumpBase->G.x, (btScalar)bumpBase->G.y, (btScalar)bumpBase->G.z));

        // Reset animations - not visible anymore - visual is lost
        //addCollisionShapes();

        mVehicleAttached = false;
        mColliderState = CS_BULLET_REGIME;

        //oapiWriteLogV("Bump_VehicleCollider::handleFrozenBullet: Moving %s to BR", strObjectName);

        // MUST return as attachment point is gone now!
        return 0;
    }

    //oapiWriteLogV("Bump_VehicleCollider::handleFrozenBullet: %s Exit", strObjectName);
    return 0;

}

/**
 * Apply thrust and get ground distance, also update wheels & collision meshes,
 * also update the ground patch.
 */
inline void
Bump_VehicleCollider::applyThrustOnDynamicVehicle(btTransform& transVessel,
                                                  btVector3& trueChassisPos,
                                                  double& magnWeight,
                                                  double& magnUpThrust)
{
    btScalar m[16];

    // Get exact distance to Touch down points(not CoG) which = rad - cogElev
    // cogElev can change based on gear up/down
	double lng, lat, rad;
    bumpBase->bb->Local2Global(_V(BT3ARGS(trueChassisPos)), gposVesselPos);
    oapiGlobalToEqu(bumpBase->hObjRefBody, gposVesselPos, &lng, &lat, &rad);


    cogElev = vCollider->GetCOG_elev();
    rad -= cogElev;

    // Here we work in vessel frame only
    VECTOR3 vdirWeight, vdirUnitWeight, vdirLift, vdirThrust, vdirDrag, upThrust;

    // TODO: Local vessel gravity must be used inside ships with artifical gravity
    vCollider->GetWeightVector(vdirWeight);
    vdirUnitWeight = unit(vdirWeight);
    magnWeight = dotp(vdirWeight, vdirUnitWeight);

    vCollider->GetLiftVector(vdirLift);
    vCollider->GetThrustVector(vdirThrust);
    vCollider->GetDragVector(vdirDrag);

    upThrust = vdirLift + vdirThrust + vdirDrag;
    magnUpThrust =  dotp(upThrust, -vdirUnitWeight);

    // TODO: Apply torque vector as GetTorque() will work now !!

    // For next iteration
    VECTOR3 fWRTVessel = upThrust/10., f;

    // Convert vector to base frame
    convertVesselDir2BaseDir(fWRTVessel, f);

    rbCollider->applyCentralForce(btVector3(V3ARGSF(f)));
    rbCollider->activate(true); //This is needed - do not remove, vehicle is de-activated otherwise

    // Rotational thrusters
    VECTOR3 torque;
    vCollider->GetTorqueVector (torque);
    VECTOR3 tWRTVessel = torque * TORQUE_MULT_FACTOR;
    VECTOR3 t;
    convertVesselDir2BaseDir(tWRTVessel, t);
    t.y = -(t.y);        // -ve thrust after direction conversion works for now
    t.x = -(t.x);
    t.z = -(t.z);
    rbCollider->applyTorqueImpulse(btVector3(V3ARGSF(t)));

    btVector3 angfac = rbCollider->getAngularFactor();

    //sprintf(oapiDebugString(), "BR : %s", strObjectName);

    //oapiWriteLogV("1. mCollisionInfo.colliding: %d", mCollisionInfo.colliding);


    // If colliding with ground patch and Orbiter reports planetary sphere contact, move to ORBITER_REGIME
	// by falsing any collision detection. We want Orbiter to handle planetary sphere collision of chassis
	if ( mCollisionInfo.colliding && isChassisOnlyInPlanetSphereContact(mCollisionInfo) ) {
		mCollisionInfo.colliding = false;
		//oapiWriteLogV("BR : 2. mCollisionInfo.colliding:%d", mCollisionInfo.colliding);
	}

    // Now check if the wheels are colliding, OR with chassis collision info from nearphase
    // m[] must not be changed before gposVesselPos is determined

    // At high time warps there is no need to check the wheels, just check the chassis
    // But wheels should not be affected by high time warps
    for (int i=0; i < mCollDef->numWheels; i++) {

    	// For wheels if Orbiter reports planetary sphere collision & the ground patch is
		// in contact then vehicle wheels are in touch with sphere, let orbiter handle it.
    	// Move to ORBITER_REGIME
		if (vehicle->getWheelInfo(i).m_raycastInfo.m_isInContact &&
            isWheelOnlyInPlanetSphereContact(vehicle->getWheelInfo(i).m_raycastInfo)
		) {
			mCollisionInfo.colliding |= 0;
			//oapiWriteLogV("BR : 3. mCollisionInfo.colliding:%d", mCollisionInfo.colliding);
		}
		else {
			// Something else, either wheel struck another object or wheels struck orulex
			// terrain high above planetary sphere. Stay in BULLET_REGIME
			mCollisionInfo.colliding |= vehicle->getWheelInfo(i).m_raycastInfo.m_isInContact;
			/*oapiWriteLogV("BR : 4. mCollisionInfo.colliding:%d, contact:%d",
					mCollisionInfo.colliding, vehicle->getWheelInfo(i).m_raycastInfo.m_isInContact);*/
		}

        // synchronize the wheels with the (interpolated) chassis worldtransform
        vehicle->updateWheelTransform(i, true);

        btTransform transWheel = vehicle->getWheelInfo(i).m_worldTransform;
        transWheel.getOpenGLMatrix(m);

        mWheelPos[i] = btVector3(m[12], m[13], m[14]) - bumpBase->simBoxOffset;

        //oapiWriteLogV("mWheelPos[%d] = (%f, %f, %f)", i, BT3ARGS(mWheelPos[i]));

        // collision may not be necessarily with the wheels, but we will
        // simulate friction using a non-zero braking force that wont effect the
        // vehicle if wheels are not in touch with the ground anyway, applied always
        //
        if (i == 0)
        	vehicle->setBrake(DEFAULT_FRICTION_BRAKING_FORCE, i);

        // Place collision meshes in Orbiter window
      /*  btTransform transShape = transWheel;
        transShape.setOrigin(mWheelPos[i]);
        mptrCollMshWheel[i]->drawSimpleShape(transShape);*/
    }


    // Update terrain to allow vessel collisions on it.
    // We must convert the physics position to equ pos and not use the lolarad from
    // vAttached->GetEquPos(lng, lat, rad) directly, as that stays fixed as long as vessel is
    // attached ! So vessel wont move if we directly use it. The physics position does change.
    if(ground){
        //bumpBase->bb->Local2Global(_V(BT3ARGS(mTrueChassisPos)), gposVesselPos);
        oapiGlobalToEqu(bumpBase->hObjRefBody, gposVesselPos, &lng, &lat, &rad);

        //oapiWriteLogV("Bump_VehicleCollider::poststep: Converted (%f,%f,%f) to lolara(%f,%f,%f)",
        //        m[12], m[13], m[14], lng, lat, rad);

        ground->updatePatch(lng, lat, rad);
    }

    //oapiWriteLogV("Friction: %f", ground->rbTerrainPatch->getFriction());

    // Update the wheel and shock absorber animations only if the vessel is not
    // in touch with planetary surface already & thus isnt already animating it.
    // Update animation in time warp mode irrespective of ground contact - this may lead to issues
    // if DG animates on sudden ground contact - this entire clause is silly, there should
    // be just 1 animation agent. - attached vessels do not get their ground contact updated
   /* if (!vCollider->GroundContact() || mColliderState == CS_TIMEWARP)
        if (mOrbiterWheelAnimationEnabled)
            updateAnimations();*/

    // Show collision mesh
  /*  btTransform transFinal = transVessel;
    transFinal.setOrigin(trueChassisPos);
    mptrCollMshChassis->drawCompoundShape(transFinal, compoundChassisShape);*/
}


/**
 * Place the dynamic vehicle above terrain.
 * This will 0 out all velocities and not fling the vehicle into space
 * It should delicately place the vehicle on the terrain such that it has
 * minimum chance of penetrating the terrain (again !).
 */
inline void
Bump_VehicleCollider::placeDynamicVehicleAboveTerrain(btTransform& transVessel)
{
    //Use motion state to correct the position
    // If collider AABB below ground, it will get stuck, it must be raised
    btVector3 aabbMinGround, aabbMaxGround;
    ground->mrbTerrainPatch->getAabb(aabbMinGround, aabbMaxGround);

    btVector3 bsphCenter;
    btScalar bsphRad;
    compoundChassisShape.getBoundingSphere(bsphCenter, bsphRad);
    btVector3 currentOrigin = transVessel.getOrigin();
    transVessel.setOrigin(btVector3(currentOrigin.x(),
                                    aabbMaxGround.y() + bsphRad,  //replace the 'y'
                                    currentOrigin.z()));

    rbCollider->setWorldTransform(transVessel);

    //oapiWriteLogV("STUCK FIX : %f, %f, %f", BT3ARGS(v));
}


inline bool
Bump_VehicleCollider::isWheelOnlyInPlanetSphereContact(btWheelInfo::RaycastInfo&	raycastInfo)
{
	/*oapiWriteLogV("isWheelOnlyInPlanetSphereContact : 5 mrbTerrainPatch: %p, mrbSupportPlate:%p",
			ground->mrbTerrainPatch, ground->mrbSupportPlate);*/

	// We come here only when a wheel reports raycast contact
	// If the hit object is not the ground patch then we are defn in contact with something
	// else, so NOT ONLY planet sphere, return false.
	if ( raycastInfo.m_groundObject != ground->mrbTerrainPatch &&
		 raycastInfo.m_groundObject != ground->mrbSupportPlate	) {
		/*oapiWriteLogV("isWheelOnlyInPlanetSphereContact : 5 m_groundObject: %p",
				raycastInfo.m_groundObject);*/
		return false;
	}

	// Verify if  Bullet will always detect ground contact if Orbiter is doing so
	// Then we can remove this as its not wheels specific
	if (vCollider->GroundContact()) {
			//oapiWriteLogV("isWheelOnlyInPlanetSphereContact : 6 true");
			return true;
	}

	// The wheel is in touch with ground patch.
	// Check for the ray hit point position only if in touch with ground patch
	if ( raycastInfo.m_groundObject == ground->mrbTerrainPatch ||
		 raycastInfo.m_groundObject == ground->mrbSupportPlate	) {
		VECTOR3 rposHitPntWRTRefBody;

		// Get the contact point wrt this vehicle's base (not Bullet WS)
		// Must annul simboxOffset & treat this base as if at 0
		btVector3 trueContactPoint = raycastInfo.m_contactPointWS - bumpBase->simBoxOffset;

		// Convert the point to the base's ref body frame
		bumpBase->bb->Local2Rel(_V(BT3ARGS(trueContactPoint)), rposHitPntWRTRefBody);

		// Now this point can be treated as a vector from ref body frame
		// origin to ray contact point. Get its length which is also the radius.
		double radHitPnt = length(rposHitPntWRTRefBody);

		//oapiWriteLogV("isWheelOnlyInPlanetSphereContact : 8 true %f < %f", radHitPnt, bumpBase->radRefBody);

		// Is the hit point below ground ? Orulex mesh is below radius of sphere & so is our ground patch
		// If it is then yes we defn have ground contact only.
		if (radHitPnt < (bumpBase->radRefBody + 0.01f) ) {
			//oapiWriteLogV("isWheelOnlyInPlanetSphereContact : 9 true %f < %f", radHitPnt, bumpBase->radRefBody);
			return true;
		}
	}

	//oapiWriteLogV("isWheelInGroundContact : 10 false");
	// Else the hit point is above ground, something else is present ?
	return false;
}


inline bool
Bump_VehicleCollider::isChassisOnlyInPlanetSphereContact(CollisionInfo& collisionInfo)
{
	// There is just 1 object colliding with chassis, if its ground then yes chassis is
	// in ground contact, we goto/stay in OR

	int mapSize = mCollisionInfo.collMap.size();
	if (mapSize > 2) {
		// There is at least 1 other object apart from ground & support plate
		// colliding with this vessel
		//oapiWriteLogV("isChassisOnlyInPlanetSphereContact : 5");
		return false;
	}

	CollisionInfo::CollisionMap::iterator collMapIter;
	if (mapSize == 2) {
		// Check if ground AND box are the 2 objects present

		// Ground
		collMapIter = mCollisionInfo.collMap.find(ground->mrbTerrainPatch);
		if(collMapIter == mCollisionInfo.collMap.end()) {
			// No it isnt ground (wasnt found in map)
			//oapiWriteLogV("isChassisOnlyInPlanetSphereContact : 6");
			return false;
		}

		// Ground is there, now check if support plate is the other one
		collMapIter = mCollisionInfo.collMap.find(ground->mrbSupportPlate);
		if(collMapIter == mCollisionInfo.collMap.end()) {
			// No it isnt support plate (wasnt found in map), some other object is colliding
			//oapiWriteLogV("isChassisOnlyInPlanetSphereContact : 7");
			return false;
		}

		// Yes both are present, must be only planet sphere contact, return true
	}

	if (mapSize == 1) {
		// Check if ground OR box is present

		// Ground
		collMapIter = mCollisionInfo.collMap.find(ground->mrbTerrainPatch);
		if(collMapIter != mCollisionInfo.collMap.end()) {
			// Ground found !! goto end and return true
			//oapiWriteLogV("isChassisOnlyInPlanetSphereContact : 7");
		}
		else {
			// Ground NOT found, check if support plate is there
			collMapIter = mCollisionInfo.collMap.find(ground->mrbSupportPlate);
			if(collMapIter == mCollisionInfo.collMap.end()) {
				// No it isnt support plate either some other object is colliding
				//oapiWriteLogV("isChassisOnlyInPlanetSphereContact : 8");
				return false;
			}
		}
	}

	//oapiWriteLogV("isChassisOnlyInPlanetSphereContact : 9");
	return true;
}


/**
 *  Process keyboard input any other messages
 */
int
Bump_VehicleCollider::processCommand()
{
    return 0;
}


/**
 * Transfer Bullet rigid body state :
 * > Position : same as attachment point release, so not needed to be transferred
 * > Orientation : transferred by the attachment point detachment angle
 * > Linear Velocity : yes, absolutely needed, to prevent a pause before Orbiter engine kicks in !
 * > Rotational Velocity : yes
 */
int
Bump_VehicleCollider::transformVessel(btVector3& trueChassisPosWS)
{
    VECTOR3 rposVesselWRTRefBody, rposBaseWRTRefBody;
    VECTOR3 rvelBaseWRTRefBody, rvelVesselWRTRefBody;
    VECTOR3 ravelVesselWRTRefBody;

    btTransform transCollider;
    //    btTransform transCollider = rbCollider->getCenterOfMassTransform();
    rbCollider->getMotionState()->getWorldTransform(transCollider);
    btMatrix3x3 mBt = transCollider.getBasis();

    // Position
    // Check if the position is being set below planet sphere - will cause vessel
    // to be thrown into space if below !!
    double lng, lat, rad;
    bumpBase->bb->Local2Rel(_V(BT3ARGS(trueChassisPosWS)), rposVesselWRTRefBody);
    oapiLocalToEqu(bumpBase->hObjRefBody, rposVesselWRTRefBody, &lng, &lat, &rad);
    //double radVesselWRTRefBody = length(rposVesselWRTRefBody);

    cogElev = vCollider->GetCOG_elev();

	// Vessel origin should be at or above (radRefBody + cog elevation) at all times
	if (rad < (bumpBase->radRefBody + cogElev) ) {
		// Modify the position to be above sphere(adding CoG too) for Orbiter
		// This position is wrt ref body
		oapiEquToLocal(bumpBase->hObjRefBody, lng, lat, (bumpBase->radRefBody + cogElev),
						                              &rposVesselWRTRefBody);

		// Now for Bullet
		VECTOR3 rposVesselWRTBase, gposChassisPos, gposMoon;
		bumpBase->bb->Local2Global(_V(BT3ARGS(trueChassisPosWS)), gposChassisPos);

		oapiGetGlobalPos(bumpBase->hObjRefBody, &gposMoon);
		VECTOR3 gdirChassisPos = gposChassisPos - gposMoon;
		gposChassisPos = unit(gdirChassisPos) * (bumpBase->radRefBody + cogElev);
		gposChassisPos += gposMoon; //translate above the moon from global origin

		// Global to base frame
		bumpBase->bb->Global2Local(gposChassisPos, rposVesselWRTBase);

		btVector3 chassisPosWS((btScalar)rposVesselWRTBase.x,
							   (btScalar)rposVesselWRTBase.y,
							   (btScalar)rposVesselWRTBase.z);
		chassisPosWS += bumpBase->simBoxOffset;
		transCollider.setOrigin(chassisPosWS);
		rbCollider->setWorldTransform(transCollider);

		//oapiWriteLogV("CS_BULLET_REGIME : DISASTER AVERTED : %f, %f, %f", V3ARGS(rposVesselWRTBase));
	}




    // Orientation
    btVector3 X = mBt.getRow(0);
    btVector3 Y = mBt.getRow(1);
    btVector3 Z = mBt.getRow(2);

    MATRIX3 mOrb = { BT3ARGS(X),
                     BT3ARGS(Y),
                     BT3ARGS(Z),
                   };

    MATRIX3 bbRot;

    bumpBase->bb->GetRotationMatrix(bbRot);
    MATRIX3 final = mul(bbRot, mOrb);
    vCollider->SetRotationMatrix(final);

    // sprintf(oapiDebugString(), "CS_BULLET_REGIME : rx:%f, ry:%f, rz:%f", rx*DEG, ry*DEG, rz*DEG);

    //oapiWriteLogV("CS_BULLET_REGIME : %f, %f, %f", BT3ARGS(trueChassisPosWS));

    // Linear velocity
    btVector3 rvelBtVessel = rbCollider->getLinearVelocity();   // base frame
    bumpBase->bb->Local2Rel( _V(BT3ARGS(rvelBtVessel)), rvelVesselWRTRefBody);

    //oapiWriteLogV("CS_BULLET_REGIME : rvelBtVessel: %f, %f, %f", BT3ARGS(rvelBtVessel));
    //oapiWriteLogV("CS_BULLET_REGIME : rvelVesselWRTRefBody: %f, %f, %f", V3ARGS(rvelVesselWRTRefBody));

    bumpBase->bb->Local2Rel(_V(0., 0., 0.), rposBaseWRTRefBody);
    bumpBase->bb->GetRelativeVel(bumpBase->hObjRefBody, rvelBaseWRTRefBody); // this is not 0

    rvelVesselWRTRefBody -= rposBaseWRTRefBody;  // to compensate for Local2Rel() converting a direction vector
    rvelVesselWRTRefBody += rvelBaseWRTRefBody;  // to account for orbital speed of base(rotating with the Moon)

    //oapiWriteLogV("CS_BULLET_REGIME : rvelVesselWRTRefBody: %f, %f, %f", V3ARGS(rvelVesselWRTRefBody));


    // Angular velocity
    btVector3 ravelBtVessel = rbCollider->getAngularVelocity();   // base frame
    bumpBase->bb->Local2Rel( _V(BT3ARGS(ravelBtVessel)), ravelVesselWRTRefBody);
    ravelVesselWRTRefBody -= rposBaseWRTRefBody;  // to compensate for Local2Rel() converting a direction vector


    // Vessel status stuff
    VESSELSTATUS2 vs;
    memset(&vs, 0, sizeof(VESSELSTATUS2));
    vs.version = 2;
    vCollider->GetStatusEx(&vs);

    // VECTOR3 ang = vs.arot;

    // sprintf(oapiDebugString(), "CS_BULLET_REGIME : rx:%f, ry:%f, rz:%f", rx*DEG, ry*DEG, rz*DEG);

    vs.status = 0;
    vs.rpos = rposVesselWRTRefBody;

    vs.rvel = rvelVesselWRTRefBody;
    vs.vrot = ravelVesselWRTRefBody;

    vCollider->DefSetStateEx(&vs);

    return 0;
}


/**
 * Create the chassis compound collision shaps just once
 *
 */
int
Bump_VehicleCollider::createCollisionShapes()
{

    if (mCollDef->frontWheelShape == NULL || mCollDef->backWheelShape == NULL) {
        oapiWriteLogV("Bump_VehicleCollider::createCollisionShapes: ERROR Wheel shapes are NULL\
                in %s", strObjectName);
        return 1;
    }

    if (mCollDef->numShapes == 0) {
        oapiWriteLogV("Bump_VehicleCollider::createCollisionShapes: ERROR Number of chassis shapes\
                is 0 in %s", strObjectName);
        return 2;
    }

    // We are all clear to use the collision shapes now !

    // Add chassis shape to compound shape
    for (int i = 0; i < mCollDef->numShapes; ++i) {
        if (mCollDef->chassisShapes[i]) {
            compoundChassisShape.addChildShape(mCollDef->chassisShapeTransforms[i],
                                               mCollDef->chassisShapes[i]);

           // oapiWriteLogV("Bump_VehicleCollider::createCollisionShapes: Added %s",
            //                    mCollDef->chassisShapes[i]->getName());
        }
        else {
            //oapiWriteLogV("Bump_VehicleCollider::createCollisionShapes: ERROR Chassis shape %d\
             //               is NULL in %s", i, strObjectName);
            return 3;
        }
    }




    return 0;
}


/**
 * Create the physics counterpart in the Bullet world
 *
 */
int
Bump_VehicleCollider::createBulletRigidBodyVehicle(bool isKinematic)
{

    bool isFrontWheel;

    // Collision shapes are ready already

    btTransform transVessel;
    getVesselTransformInBaseFrame(transVessel);
    mass = (btScalar)vCollider->GetMass();

    btVector3 v = transVessel.getOrigin();
    //oapiWriteLogV("createBulletRigidBodyVehicle : %s ORIGIN %f, %f, %f", strObjectName, BT3ARGS(v));

    VECTOR3 pmi;
    vCollider->GetPMI(pmi);
    btVector3 vesselInertia(V3ARGSF(pmi));

    if(isKinematic) {
        rbCollider = phyEngine->addKinematicBody(1.0f, transVessel, &compoundChassisShape);
    }
    else {
    	// The vehicle action requires a non-zero mass for a kinematic body too
		btVector3 aabbMinGround, aabbMaxGround;
		ground->mrbTerrainPatch->getAabb(aabbMinGround, aabbMaxGround);
		// oapiWriteLogV("rbCollider: %f, grnd:%f", aabbMin.y(), aabbMaxGround.y());

		// If collider AABB below ground, it will get stuck, it must be raised
	/*	if(aabbMin.y() <  aabbMaxGround.y()){
			oapiWriteLogV("Bump_VehicleCollider::createBulletRigidBodyVehicle: WARN : Collider may get STUCK : %s", strObjectName);
			btVector3 bsphCenter;
			btScalar bsphRad;
			compoundChassisShape.getBoundingSphere(bsphCenter, bsphRad);
			transVessel.setOrigin(btVector3(v.x(), aabbMaxGround.y() + bsphRad, v.z()));

			v = transVessel.getOrigin();
			oapiWriteLogV("Bump_VehicleCollider::createBulletRigidBodyVehicle: WARN : STUCK FIXED : %f, %f, %f", BT3ARGS(v));
		}*/

        rbCollider = phyEngine->addRigidBody(mass/10.f, transVessel, &compoundChassisShape, vesselInertia);
    }

   /* oapiWriteLogV("Bump_VehicleCollider::createBulletRigidBodyVehicle: %s rbCollider : %p",
    		strObjectName, rbCollider);*/

    // Set initial velocities
    rbCollider->setLinearVelocity(linearVel);
    rbCollider->setAngularVelocity(angularVel);

    rbCollider->setActivationState(DISABLE_DEACTIVATION); // this may cause issues if frozen by deactivation


    // Collision structure
    rbCollider->setUserPointer(&mCollisionInfo);




    // Create vehicle
    vehicleRayCaster = new btDefaultVehicleRaycaster(bumpBase->m_dynamicsWorld);
    vehicle = new btRaycastVehicle(wheelTuning, rbCollider, vehicleRayCaster);
    bumpBase->m_dynamicsWorld->addVehicle(vehicle);

    // Get TDPs in vessel frame
    VECTOR3 pt1, pt2, pt3;
    vCollider->GetTouchdownPoints(pt1, pt2, pt3);

    // choose coordinate system
    vehicle->setCoordinateSystem(mCollDef->rightIndex, mCollDef->upIndex, mCollDef->forwardIndex);

    // Forward wheel
    isFrontWheel=true;
    btVector3 connectionPoint = btVector3((btScalar)pt1.x, mCollDef->frontConnectionHeight, (btScalar)pt1.z /*- 1.0f*/);
    vehicle->addWheel(connectionPoint,
                    mCollDef->wheelDirection,
                    mCollDef->wheelAxle,
                    mCollDef->frontSuspensionRestLength,
                    mCollDef->frontWheelRadius,
                    wheelTuning,
                    isFrontWheel);

    isFrontWheel = false;
    // Backward left wheel
    connectionPoint = btVector3((btScalar)pt2.x, mCollDef->backConnectionHeight + 0.1f, (btScalar)pt2.z);
    vehicle->addWheel(connectionPoint,
                    mCollDef->wheelDirection,
                    mCollDef->wheelAxle,
                    mCollDef->backSuspensionRestLength,
                    mCollDef->backWheelRadius,
                    wheelTuning,
                    isFrontWheel);

    // Backward right wheel
    connectionPoint = btVector3((btScalar)pt3.x, mCollDef->backConnectionHeight + 0.1f, (btScalar)pt3.z);
    vehicle->addWheel(connectionPoint,
                    mCollDef->wheelDirection,
                    mCollDef->wheelAxle,
                    mCollDef->backSuspensionRestLength,
                    mCollDef->backWheelRadius,
                    wheelTuning,
                    isFrontWheel);

    // Set wheel properties for all wheels
    for (int i=0; i < mCollDef->numWheels; i++){

        btWheelInfo& wheel = vehicle->getWheelInfo(i);

        if (wheel.m_bIsFrontWheel) {
            wheel.m_suspensionStiffness = mCollDef->frontSuspensionStiffness;
            wheel.m_wheelsDampingRelaxation = mCollDef->frontSuspensionDamping;
            wheel.m_wheelsDampingCompression = mCollDef->frontSuspensionCompression;
            wheel.m_frictionSlip = mCollDef->frontWheelFriction;
            wheel.m_rollInfluence = mCollDef->rollInfluence;
        }
        else {
            wheel.m_suspensionStiffness = mCollDef->backSuspensionStiffness;
            wheel.m_wheelsDampingRelaxation = mCollDef->backSuspensionDamping;
            wheel.m_wheelsDampingCompression = mCollDef->backSuspensionCompression;
            wheel.m_frictionSlip = mCollDef->backWheelFriction;
            wheel.m_rollInfluence = mCollDef->rollInfluence;
        }
    }

    // Celestial body specific gravity
    rbCollider->setGravity(btVector3((btScalar)bumpBase->G.x, (btScalar)bumpBase->G.y, (btScalar)bumpBase->G.z));


    // rbCollider->setGravity(btVector3(0., -10, 0.));

    // Get the defaut flags - this is a good place to get it after the vehicle has been added to the dyn. world
    if (isKinematic) {
    	rbCollider->setCollisionFlags( rbCollider->getCollisionFlags() | btCollisionObject::CF_KINEMATIC_OBJECT);
    }



    // Wheel rendering - called by visualizer thread
    m_ShapeDrawer = phyEngine->getShapeDrawer();
    phyEngine->addExternalRenderObject(this, renderFuncPos);

    //oapiWriteLogV("Valid functions allocated : %x", renderFuncPos);



    //addCollisionShapes();

    //rbCollider->setCcdMotionThreshold(1);

    return 0;
}


void
Bump_VehicleCollider::addCollisionShapes()
{
    if(!mCollisionMeshActive) {

        // Set the collision mesh
        char meshName[MAX_STRLEN];

        mptrCollMshWheel[0] = new Bump_CollisionShape();
        mptrCollMshWheel[1] = new Bump_CollisionShape();
        mptrCollMshWheel[2] = new Bump_CollisionShape();
        mptrCollMshChassis  = new Bump_CollisionShape();


        sprintf(meshName, "%s_Wheel%d", strObjectName, 0);
        mptrCollMshWheel[0]->addSimpleShape(bumpBase, mCollDef->frontWheelShape, meshName);


        sprintf(meshName, "%s_Wheel%d", strObjectName, 1);
        mptrCollMshWheel[1]->addSimpleShape(bumpBase, mCollDef->backWheelShape, meshName);

        sprintf(meshName, "%s_Wheel%d", strObjectName, 2);
        mptrCollMshWheel[2]->addSimpleShape(bumpBase, mCollDef->backWheelShape, meshName);


        sprintf(meshName, "%s_Chassis", strObjectName);
        //btVector3 halfExtent = (static_cast<const btBoxShape*>(chassisShape))->getHalfExtentsWithoutMargin();
        //oapiWriteLogV("Halves in cpb : %f, %f, %f", BT3ARGS(halfExtent));
        mptrCollMshChassis->addCompoundShape(bumpBase, &compoundChassisShape, meshName, mCollDef->chassisShapeFileNames);
        mCollisionMeshActive = true;

    /*    // TDP markers
        vCollider->AddMesh("Bump/DebugMeshes/LittleArrow", &pt1);
        vCollider->AddMesh("Bump/DebugMeshes/LittleArrow", &pt2);
        vCollider->AddMesh("Bump/DebugMeshes/LittleArrow", &pt3);*/

        /*oapiWriteLogV("TDP: (%f, %f, %f), (%f, %f, %f), (%f, %f, %f)",
                V3ARGS(pt1), V3ARGS(pt2), V3ARGS(pt3));*/
    }

}


/**
 * Remove collision shapes
 */
void
Bump_VehicleCollider::removeCollisionShapes()
{
    if (! bumpBase->simIsEnding && mCollisionMeshActive) {
        delete mptrCollMshWheel[0];
        delete mptrCollMshWheel[1];
        delete mptrCollMshWheel[2];
        delete mptrCollMshChassis;
        mCollisionMeshActive = false;
    }
}


/*
 * Wheels rendering for the vehicle
 */
int
Bump_VehicleCollider::clbkPhysicsRender(void)
{
    int i;
    btScalar m[16];
    btVector3 wheelColor(1,0,0);
    btVector3    worldBoundsMin,worldBoundsMax;
    bumpBase->m_dynamicsWorld->getBroadphase()->getBroadphaseAabb(worldBoundsMin,worldBoundsMax);


    for (i=0;i<vehicle->getNumWheels();i++)
    {
        // synchronize the wheels with the (interpolated) chassis worldtransform
        vehicle->updateWheelTransform(i,true);
        // draw wheels (cylinders)
        vehicle->getWheelInfo(i).m_worldTransform.getOpenGLMatrix(m);
        if(i==0)
            m_ShapeDrawer->drawOpenGL(m, mCollDef->frontWheelShape,wheelColor, phyEngine->getDebugMode(),worldBoundsMin,worldBoundsMax);
        else
            m_ShapeDrawer->drawOpenGL(m, mCollDef->backWheelShape,wheelColor, phyEngine->getDebugMode(),worldBoundsMin,worldBoundsMax);

    }

    return 0;
}


/**
 * De-allocates memory being consumed by the raycast vehicle so it can be
 * created again by createBulletRigidBody()
 *
 * This is a very important function. If memory is not freed up, Orbiter sim will not
 * exit properly and the Launchpad will not appear leading to user frustration!
 *
 * Here we de-allocate only the sub class specific stuff which have not been freed by
 * phyEngine.clientResetScene(), which is called before Bump_BulletBase::exit();
 *
 * Thus rigid bodies(derived from CollisionObjects), motion states are already removed
 * by the time we come here, DO NOT remove them again !
 *
 * The stuff which should be removed are any sub class specific classes used to model
 * this object like m_vehicleRayCaster & vehicle. Also any collision shapes not pushed
 * into m_collisionShapes[] during init(), needs to be freed here, such as the frontWheelShape
 */
int
Bump_VehicleCollider::destroyBulletRigidBodyVehicle()
{
    //oapiWriteLog("Bump_VehicleCollider::destroyBulletRigidBodyVehicle: Entering !");

    rbCollider->getAabb(aabbMin, aabbMax);

    // Only if this object has focus, should we shift focus to base in Visualizer
    if(oapiGetFocusObject() == hObjThis)
        bumpBase->setCameraFocus(VCM_ORBITER); // TODO : default value should be VCC_ORBITER

    //oapiWriteLogV("Destroying : %p", rbCollider);


    // Remove this registered object from rendering list, as it will be destroyed after this
    phyEngine->removeExternalRenderObject(renderFuncPos);

    // THIS IS REQUIRED FOR VEHICLES, FIRST
    bumpBase->m_dynamicsWorld->removeVehicle(vehicle);

    // Manually delete the collision shape instead of adding it to the sim wide list
    if(rbCollider && rbCollider->getMotionState()) {
        delete rbCollider->getMotionState();
    }

    btCollisionObject *obj = dynamic_cast<btCollisionObject*>(rbCollider);
    bumpBase->m_dynamicsWorld->removeCollisionObject(obj);
    delete obj;


    // Vehicle specific objects
    delete vehicleRayCaster;
    delete vehicle;

    //removeCollisionShapes();

    //oapiWriteLog("Exiting destroy");
    return 0;
}


/**
 * Create the wheel animations. Wheel animations are needed to make the wheels 'bounce'
 * on contact with Orulex terrain - Orbiter does not bounce them as Orulex terrain is
 * not detected by Orbiter.
 *
 * Required only during Bullet regime when there is surface contact, not Orbiter regime
 */
void
Bump_VehicleCollider::createAnimations(void)
{
    // Get the original wheel centers
    for (int i = 0; i < mCollDef->numWheels; i++)
        mWheelMeshGroupCenter[i] = mCollDef->wheelMeshGroupCenter[i];

}


/**
 * Update the wheel positions
 */
void
Bump_VehicleCollider::updateAnimations(void)
{
    for (int i = 0; i < mCollDef->numWheels; i++) {

        VECTOR3 rposWheel = _V(BT3ARGS(mWheelPos[i]));
        VECTOR3 vposWheel;
        convertBasePos2VesselPos(rposWheel, vposWheel);

    /*    oapiWriteLogV("mWheelPos (%f, %f, %f)", BT3ARGS(mWheelPos[i]));
        oapiWriteLogV("mWheelMeshGroupCenter (%f, %f, %f)", V3ARGS(mWheelMeshGroupCenter[i]));
        oapiWriteLogV("vpos (%f, %f, %f)", V3ARGS(vposWheel));*/




        VECTOR3 delta = vposWheel - mWheelMeshGroupCenter[i];
        delta.x = 0;  // x & z are not in sync, 0 them out
        delta.z = 0;

        //oapiWriteLogV("delta (%f, %f, %f)", V3ARGS(delta));

        VISHANDLE hvisCar = *oapiObjectVisualPtr (oapiGetObjectByName(strObjectName));

        if (hvisCar != NULL) {
            MESHGROUP_TRANSFORM mt;

            // Delta translation of wheel
            mt.P.transparam.shift = delta;
            mt.transform = mt.TRANSLATE;
            mt.ngrp = mCollDef->wheelMeshGroupIndex[i];
            mt.nmesh = mCollDef->wheelMeshIndex;
            bumpBase->bb->MeshgroupTransform(hvisCar, mt);

            // Delta translation of struts - same as the wheels, so no mesh center info needed
            mt.P.transparam.shift = delta;
            mt.transform = mt.TRANSLATE;
            mt.ngrp = mCollDef->wheelStrutMeshGroupIndex[i];
            mt.nmesh = mCollDef->wheelMeshIndex;
            bumpBase->bb->MeshgroupTransform(hvisCar, mt);

            // Update to the new center after delta translation
            mWheelMeshGroupCenter[i] = vposWheel;

        }

    }


}


/**
 * Restore the wheels and structs to their original positions
 *
 * Do not factorize this function and updateAnimations(). There are subtle
 * differences in the co-ordinate systems. mCollDef->wheelMeshGroupCenter[] is in
 * vehicle co-ord system.
 */
void
Bump_VehicleCollider::deleteAnimations(void)
{
    // Restore original mesh positions
    for (int i = 0; i < mCollDef->numWheels; i++) {


        //oapiWriteLogV("mCollDef->wheelMeshGroupCenter[i] (%f, %f, %f)", V3ARGS(mCollDef->wheelMeshGroupCenter[i]));
        //oapiWriteLogV("mWheelMeshGroupCenter (%f, %f, %f)", V3ARGS(mWheelMeshGroupCenter[i]));


        VECTOR3 delta = mCollDef->wheelMeshGroupCenter[i] - mWheelMeshGroupCenter[i];
        delta.x = 0;  // x & z are not in sync, 0 them out
        delta.z = 0;

        //oapiWriteLogV("delta (%f, %f, %f)", V3ARGS(delta));

        VISHANDLE hvisCar = *oapiObjectVisualPtr (oapiGetObjectByName(strObjectName));

        if (hvisCar != NULL) {
            MESHGROUP_TRANSFORM mt;

            // Delta translation of wheel
            mt.P.transparam.shift = delta;
            mt.transform = mt.TRANSLATE;
            mt.ngrp = mCollDef->wheelMeshGroupIndex[i];
            mt.nmesh = mCollDef->wheelMeshIndex;
            bumpBase->bb->MeshgroupTransform(hvisCar, mt);

            // Delta translation of struts - same as the wheels, so no mesh center info needed
            mt.P.transparam.shift = delta;
            mt.transform = mt.TRANSLATE;
            mt.ngrp = mCollDef->wheelStrutMeshGroupIndex[i];
            mt.nmesh = mCollDef->wheelMeshIndex;
            bumpBase->bb->MeshgroupTransform(hvisCar, mt);

            // Update to the new center after delta translation
            mWheelMeshGroupCenter[i] = mCollDef->wheelMeshGroupCenter[i];

        }

    }
}


/**
 * Attach the VESSEL to the BulletBase in the Orbiter world
 *
 */
int
Bump_VehicleCollider::attachOrbiterVESSEL()
{
    char strAttID[8];
    VECTOR3 rdirXaxis, rdirUp, rdirForw;

    //Get the vessel axes in base frame to attach vessel in base frame correctly
    convertVesselDir2BaseDir(_V(1.f, 0.f, 0.f), rdirXaxis);  //x
    convertVesselDir2BaseDir(_V(0.f, 1.f, 0.f), rdirUp);     //y
    convertVesselDir2BaseDir(_V(0.f, 0.f, 1.f), rdirForw);   //z

    //Create an attachment point at the base, sep parent/child lists exist,  take next avail. pos.
    sprintf(strAttID, "BB%d", bumpBase->bb->AttachmentCount(FALSE));
    mAHBase = bumpBase->bb->CreateAttachment(false,
            rposVesselPos,
            rdirUp,
            rdirForw,
            strAttID);


    //Create an attachment point for the VESSEL    , -1 temp. for debug.
    mAHVehicle = vCollider->CreateAttachment(true, _V(0, 0, 0),
                                                 _V(0, -1, 0),
                                                 _V(0, 0, 1),
                                                 "BBA_CHILD");

    //Attachment points are valid ?
    if(mAHBase && mAHVehicle){

        //Try attaching, child hObj is first parameter
        if(bumpBase->bb->AttachChild(hObjThis, mAHBase, mAHVehicle)){

            //Success!, attached
        /*    oapiWriteLogV("Bump_VesselWrapper::attachOrbiterVESSEL: Attached %s to %s at attachment handle position %d",
                                strObjectName, bumpBase->strBaseName, bumpBase->bb->AttachmentCount(FALSE));
            */

            //Set vessel status to landed to bypass ranging
            VESSELSTATUS2 vs;
            memset(&vs, 0, sizeof(VESSELSTATUS2));
            vs.version = 2;

            vCollider->GetStatusEx(&vs);
            vs.status = 1;
            vCollider->DefSetStateEx(&vs);

            //Return non-zero only if object was found and attached, anything else must return 1
            return 0;
        }
        else{ //Failure, could not attach
            oapiWriteLogV("Bump_VehicleCollider::attachOrbiterVESSEL: ERROR : Attach failed: p:%d c:%d &p:%p, &c:%p",
                    bumpBase->bb->AttachmentCount(FALSE),
                    vCollider->AttachmentCount(TRUE),
                    mAHBase,
                    mAHVehicle);

            //Should NULL all imp ptrs, to prevent use
            mAHBase = NULL;
            mAHVehicle = NULL;

            return 1; //attaching failure
        }



    }
    else{
        //One of the attachment handles could not be created
        oapiWriteLogV("Bump_VehicleCollider::attachOrbiterVESSEL: ERROR : Attachment handle creation failed: p:%d c:%d &p:%p, &c:%p",
                                    bumpBase->bb->AttachmentCount(FALSE),
                                    vCollider->AttachmentCount(TRUE),
                                    mAHBase,
                                    mAHVehicle);
    }

    mAHBase = NULL;
    mAHVehicle = NULL;

    //If we reach here, the vessel was not found
    return 2;
}


/**
 * Detach the VESSEL from the BulletBase in the Orbiter world
 *
 */
int
Bump_VehicleCollider::detachOrbiterVESSEL()
{

    //Attachment points are valid ?
    if(mAHBase && mAHVehicle){

        //Try detaching child
        if(bumpBase->bb->DetachChild(mAHBase)){
            //Success!, detached
            oapiWriteLogV("Bump_VehicleCollider::detachOrbiterVESSEL: Detached VESSEL %s from %s, "\
                    "attachment count in base now %d",
                    strObjectName, bumpBase->strBaseName, bumpBase->bb->AttachmentCount(FALSE));

            //Delete the attachment handles
            if(!(bumpBase->bb->DelAttachment(mAHBase)) || !(vCollider->DelAttachment(mAHVehicle))){
                oapiWriteLogV("Bump_VehicleCollider::init: ERROR : Deletion failed for attachment point: p:%d c:%d &p:%p, &c:%p",
                                    bumpBase->bb->AttachmentCount(FALSE),
                                    vCollider->AttachmentCount(TRUE),
                                    mAHBase,
                                    mAHVehicle);
            }


            //These must be nulled after successful detach ONLY
            mAHBase = NULL;
            mAHVehicle = NULL;


        }
        else{ //Failure, could not detach
            oapiWriteLogV("Bump_VehicleCollider::init: ERROR : Detach failed: p:%d c:%d &p:%p, &c:%p",
                    bumpBase->bb->AttachmentCount(FALSE),
                    vCollider->AttachmentCount(TRUE),
                    mAHBase,
                    mAHVehicle);

            return 1;
        }

    }
    else{
        //One of the attachment handles is not valid, cannot detach
        oapiWriteLogV("Bump_VehicleCollider::init: ERROR : Detachment failed due to invalid attachment handles: p:%d c:%d &p:%p, &c:%p",
                                    bumpBase->bb->AttachmentCount(FALSE),
                                    vCollider->AttachmentCount(TRUE),
                                    mAHBase,
                                    mAHVehicle);
        return 1;
    }

    //oapiWriteLog("Exiting detach");
    return 0;
}
